using BepuPhysics.Collidables;
using BepuUtilities;
using System;
using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics.CollisionDetection.CollisionTasks
{
    public struct CapsulePairTester : IPairTester<CapsuleWide, CapsuleWide, Convex2ContactManifoldWide>
    {
        public int BatchSize => 32;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Test(
            ref CapsuleWide a, ref CapsuleWide b, ref Vector<float> speculativeMargin,
            ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount,
            out Convex2ContactManifoldWide manifold)
        {
            // 计算两条直线段之间最近的点。一开始就不要夹紧。
            // 我们希望最小化距离=||(a+da*ta)-(b+db*tb)||。
            // 计算关于ta的导数并进行一些代数运算(考虑||da||=||db||==1)来求解ta的结果：
            // Ta=(da*(b-a)+(db*(a-b))*(da*db))/(1-((da*db)*(da*db)
            QuaternionWide.TransformUnitXY(orientationA, out var xa, out var da);
            QuaternionWide.TransformUnitY(orientationB, out var db);
            Vector3Wide.Dot(da, offsetB, out var daOffsetB);
            Vector3Wide.Dot(db, offsetB, out var dbOffsetB);
            Vector3Wide.Dot(da, db, out var dadb);
            // 请注意,当轴线平行时,电势除以零。任意钳位;接近零值会产生极值,而这些极值被钳位到合理的结果。
            var ta = (daOffsetB - dbOffsetB * dadb) / Vector.Max(new Vector<float>(1e-15f), Vector<float>.One - dadb * dadb);
            // tb=ta*(da*db)-db*(b-a)
            var tb = ta * dadb - dbOffsetB;

            // 我们不能简单地将ta和tb值夹在胶囊线段上。取而代之的是,将每条线段投影到另一条线段上,夹紧目标的间隔。
            // 该新的钳制投影间隔是该直线段上的有效解空间。我们可以用这个区间钳制t值,得到正确的有界解。
            // 预计间隔为：
            // B到A：+-BHalfLength*(da*db)+da*offsetB
            // A到B：+-AHalfLength*(da*db)-db*offsetB
            var absdadb = Vector.Abs(dadb);
            var bOntoAOffset = b.HalfLength * absdadb;
            var aOntoBOffset = a.HalfLength * absdadb;
            var aMin = Vector.Max(-a.HalfLength, Vector.Min(a.HalfLength, daOffsetB - bOntoAOffset));
            var aMax = Vector.Min(a.HalfLength, Vector.Max(-a.HalfLength, daOffsetB + bOntoAOffset));
            var bMin = Vector.Max(-b.HalfLength, Vector.Min(b.HalfLength, -aOntoBOffset - dbOffsetB));
            var bMax = Vector.Min(b.HalfLength, Vector.Max(-b.HalfLength, aOntoBOffset - dbOffsetB));
            ta = Vector.Min(Vector.Max(ta, aMin), aMax);
            tb = Vector.Min(Vector.Max(tb, bMin), bMax);

            Vector3Wide.Scale(da, ta, out var closestPointOnA);
            Vector3Wide.Scale(db, tb, out var closestPointOnB);
            Vector3Wide.Add(closestPointOnB, offsetB, out closestPointOnB);
            // 请注意,按照惯例,法线被校准为从B指向A。
            Vector3Wide.Subtract(closestPointOnA, closestPointOnB, out manifold.Normal);
            Vector3Wide.Length(manifold.Normal, out var distance);
            var inverseDistance = Vector<float>.One / distance;
            Vector3Wide.Scale(manifold.Normal, inverseDistance, out manifold.Normal);
            // 在线段接触的情况下,法线不存在,我们需要另一种选择。沿任一胶囊的局部水平(XZ)平面的任意方向
            // 是有效的。(由于非零线长度,沿局部Y轴的法线不能保证与路径分离的速度一样快。)
            var normalIsValid = Vector.GreaterThan(distance, new Vector<float>(1e-7f));
            Vector3Wide.ConditionalSelect(normalIsValid, manifold.Normal, xa, out manifold.Normal);

            // 在两个胶囊轴线共面的情况下,我们接受整个间隔作为接触源。
            // 当轴线偏离共面时,可接受的间隔迅速缩小到零长度,以ta和Tb为中心。
            // 我们根据胶囊轴线与由相对管段和接触法线定义的平面之间的角度来评定共面程度：
            // sin(角度)=点(da,(Db X Normal)/||db x Normal||)
            // 最后,请注意,我们处理的是极小的角度,并且对于小角度sin(Angle)~=角度,
            // 而且淡入淡出的行为是完全任意的,所以我们可以直接使用平方角,而不用担心。
            // 角度^2~=点(da,(Db X Normal))^2/||db x Normal|^2
            // 请注意,如果||db x Normal||为零,则任何da都应被接受为共面,因为没有限制。条件选择不连续的部分。
            Vector3Wide.CrossWithoutOverlap(db, manifold.Normal, out var planeNormal);
            Vector3Wide.LengthSquared(planeNormal, out var planeNormalLengthSquared);
            Vector3Wide.Dot(da, planeNormal, out var numeratorUnsquared);
            var squaredAngle = Vector.ConditionalSelect(Vector.LessThan(planeNormalLengthSquared, new Vector<float>(1e-10f)), Vector<float>.Zero, numeratorUnsquared * numeratorUnsquared / planeNormalLengthSquared);

            // 将平方角度转换为LERP参数。对于从0到LowerThreshold的平方角度,我们应该使用完整的间隔(1)。从lowerThreshold到upperThreshold,将LERP设置为0。
            const float lowerThresholdAngle = 0.01f;
            const float upperThresholdAngle = 0.05f;
            const float lowerThreshold = lowerThresholdAngle * lowerThresholdAngle;
            const float upperThreshold = upperThresholdAngle * upperThresholdAngle;
            var intervalWeight = Vector.Max(Vector<float>.Zero, Vector.Min(Vector<float>.One, (new Vector<float>(upperThreshold) - squaredAngle) * new Vector<float>(1f / (upperThreshold - lowerThreshold))));
            // 如果线段相交,即使它们是共面的,理想情况下我们也会坚持使用单个点。会很容易,
            // 但我们不费心,因为这是一个非常奇怪和非常暂时的角落案件。真的不值得处理。
            var weightedTa = ta - ta * intervalWeight;
            aMin = intervalWeight * aMin + weightedTa;
            aMax = intervalWeight * aMax + weightedTa;

            Vector3Wide.Scale(da, aMin, out manifold.OffsetA0);
            Vector3Wide.Scale(da, aMax, out manifold.OffsetA1);
            // 在共面的情况下,有两个点。我们需要一种计算深度的方法,给第二次接触一个合理的结果。
            // 请注意,两个触点中的一个最终的距离应该等于之前计算的线段距离,因此我们在这里做了一些多余的工作。
            // 做这项额外的工作比跟踪哪个端点贡献了更低的距离要容易得多。
            // 取消最终间隔端点从a到b的投影。
            // 点(偏移B+db*tb0,da)=TA0
            // TB0=(TA0-daOffsetB)/dadb
            // 距离0=点(a0-(偏移B+tb0*db),正常)
            // 距离1=点(a1-(偏移B+tb1*db),正常)
            Vector3Wide.Dot(db, manifold.Normal, out var dbNormal);
            Vector3Wide.Subtract(manifold.OffsetA0, offsetB, out var offsetB0);
            Vector3Wide.Subtract(manifold.OffsetA1, offsetB, out var offsetB1);
            // 注意电势除以零。在这种情况下,将两个投影点视为最近的点。(由选择先前计算的距离的条件选择处理。)
            var inverseDadb = Vector<float>.One / dadb;
            var projectedTb0 = Vector.Max(bMin, Vector.Min(bMax, (aMin - daOffsetB) * inverseDadb));
            var projectedTb1 = Vector.Max(bMin, Vector.Min(bMax, (aMax - daOffsetB) * inverseDadb));
            Vector3Wide.Dot(offsetB0, manifold.Normal, out var b0Normal);
            Vector3Wide.Dot(offsetB1, manifold.Normal, out var b1Normal);
            var capsulesArePerpendicular = Vector.LessThan(Vector.Abs(dadb), new Vector<float>(1e-7f));
            var distance0 = Vector.ConditionalSelect(capsulesArePerpendicular, distance, b0Normal - dbNormal * projectedTb0);
            var distance1 = Vector.ConditionalSelect(capsulesArePerpendicular, distance, b1Normal - dbNormal * projectedTb1);
            var combinedRadius = a.Radius + b.Radius;
            manifold.Depth0 = combinedRadius - distance0;
            manifold.Depth1 = combinedRadius - distance1;

            // 将法线偏移应用于接触位置。
            var negativeOffsetFromA0 = manifold.Depth0 * 0.5f - a.Radius;
            var negativeOffsetFromA1 = manifold.Depth1 * 0.5f - a.Radius;
            Vector3Wide.Scale(manifold.Normal, negativeOffsetFromA0, out var normalPush0);
            Vector3Wide.Scale(manifold.Normal, negativeOffsetFromA1, out var normalPush1);
            Vector3Wide.Add(manifold.OffsetA0, normalPush0, out manifold.OffsetA0);
            Vector3Wide.Add(manifold.OffsetA1, normalPush1, out manifold.OffsetA1);
            manifold.FeatureId0 = Vector<int>.Zero;
            manifold.FeatureId1 = Vector<int>.One;
            var minimumAcceptedDepth = -speculativeMargin;
            manifold.Contact0Exists = Vector.GreaterThanOrEqual(manifold.Depth0, minimumAcceptedDepth);
            manifold.Contact1Exists = Vector.BitwiseAnd(
                Vector.GreaterThanOrEqual(manifold.Depth1, minimumAcceptedDepth),
                Vector.GreaterThan(aMax - aMin, new Vector<float>(1e-7f) * a.HalfLength));

            // TODO：因为我们增加了2个联系支持的复杂性,所以这可能足够大,可以在其中一个胶囊的本地空间中工作。
            // 值得以后再去看看。
        }

        public void Test(ref CapsuleWide a, ref CapsuleWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold)
        {
            throw new NotImplementedException();
        }

        public void Test(ref CapsuleWide a, ref CapsuleWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex2ContactManifoldWide manifold)
        {
            throw new NotImplementedException();
        }
    }
}
